#include <Wire.h>
#include <ICM20948_WE.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <math.h>
#include <Arduino.h>
#include <WiFi.h>
#include <ESP32Servo.h>
#include "imu.h"
#include "MyProtocol.h"
#define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
#define AD0_VAL 0
#define ICM20948_ADDR 0x68
ICM20948_WE myIMU = ICM20948_WE(ICM20948_ADDR);
xyzFloat accRaw;//加速度计原始数据
xyzFloat accoffset;//加速度计零飘校正数据
xyzFloat accdata;//加速度计校正后数据
xyzFloat gyrRaw;//陀螺仪原始数据
xyzFloat gyroffset;//陀螺仪零飘校正数据
xyzFloat gyrdata;//陀螺仪校正后数据
const char *wifi_SSID="YXDZ_ESP32";  //保存AP的名称信息
const char *wifi_Password="ESP32123";  //保存AP的密码信息
bool IMU_init_flag=false;//IMU加速度计辅助初始化标志位
// 目标UDP服务器IP和端口
const char* udpAddress = "192.168.4.2"; // 目标设备的IP地址
const int udpPort = 7890;
unsigned int localUdpPort = 2333; //本地端口号

// UDP实例
WiFiUDP udp;

const uint16_t serverPort = 1234; //端口
WiFiClient client;

double pid_params_in[9];
double pid_params_out[9];

struct PacketHeader {
  uint32_t packetType;   // 数据报类型
  uint32_t totalPackets; // 总包数
  uint32_t currentIndex; // 当前包序号
};

void setup() {

  imu_x = 0;
  imu_y = 90;
  imu_z = 0;

  delay(2000);
  Wire.begin();
  delay(2000);

  motor_set();//电机初始化

  wifi_set();
  Serial.begin(115200);
    if(!myIMU.init()){
    Serial.println("ICM20948 does not respond");
  }
  else{
    Serial.println("ICM20948 is connected");
  }
   myIMU.setAccRange(ICM20948_ACC_RANGE_2G);//设置量程
   myIMU.setGyrRange(ICM20948_GYRO_RANGE_250);
   myIMU.setAccDLPF(ICM20948_DLPF_6);//设置滤波器等级
   myIMU.setGyrDLPF(ICM20948_DLPF_6); 
   myIMU.setAccSampleRateDivider(0);//设置输出速率
   myIMU.setGyrSampleRateDivider(0);
  Serial.println("Position your ICM20948 flat and don't move it - calibrating...");
  delay(1000);
  autooffset(&accoffset);
  Serial.println("Done!");
  xTaskCreatePinnedToCore(acctask, "acctask", 1024 * 4, NULL, 1, NULL,1);
  xTaskCreatePinnedToCore(gyrtask, "gyrtask", 1024 * 4, NULL, 1, NULL,1);
  xTaskCreatePinnedToCore(wifitask, "wifitask", 1024 * 4, NULL, 1, NULL,1);
  xTaskCreatePinnedToCore(vehicletask, "vehicletask", 1024 * 4, NULL, 1, NULL,1);
  xTaskCreatePinnedToCore(solvetask, "solvetask", 1024 * 4, NULL, 3, NULL,1);
}
void motor_set(){
  myservo1.setPeriodHertz(50);
  myservo2.setPeriodHertz(50);
  myservo1.attach(servo_pin1,1000,2000); // attaches the servo on pin 18 to the servo object
  myservo2.attach(servo_pin2,1000,2000); // attaches the servo on pin 18 to the servo object
  myservo3.setPeriodHertz(50);    // standard 50 hz servo
  myservo3.attach(servo_pin3, 900, 2100); // attaches the servo on pin 18 to the servo object
  myservo4.setPeriodHertz(50);    // standard 50 hz servo
  myservo4.attach(servo_pin4, 900, 2100); // attaches the servo on pin 18 to the servo object  
}
void wifi_set(){
  // 启动WiFi为AP模式
  WiFi.softAP(wifi_SSID, wifi_Password);

  // 输出AP的相关信息到串口
  Serial.print("\nESP32建立的WiFi名称为:");
  Serial.println(wifi_SSID);
  Serial.print("ESP32建立WiFi的IP地址为:");
  Serial.println(WiFi.softAPIP());

  // 启动UDP监听
  udp.begin(localUdpPort);
  Serial.printf("UDP监听端口:%d\n", localUdpPort);
  WIRE_PORT.begin();
  WIRE_PORT.setClock(400000);

}
void loop() {
 
}
void autooffset(xyzFloat *accoffsetVal)
{
  /*
    这里是计算初始零飘
  */
  for(int i=0;i<1000;i++)
  {
      myIMU.readSensor();
      myIMU.getAccRawValues(&accRaw);
      myIMU.getGyrRawValues(&gyrRaw);
      accoffset.x += accRaw.x;
      accoffset.y += accRaw.y;
      accoffset.z += accRaw.z;
      gyroffset.x +=gyrRaw.x;
      gyroffset.y +=gyrRaw.y;
      gyroffset.z +=gyrRaw.z;
      delay(10);
  }
    accoffset.x /= 1000;
    accoffset.y /= 1000;
    accoffset.z /= 1000;
    gyroffset.x /= 1000;
    gyroffset.y /= 1000;
    gyroffset.z /= 1000;
    accoffset.y -= 16384.0;
}
void getaccdata(xyzFloat *accoffsetVal,xyzFloat *accdataVal)
{
  /*
  输出加速度数据单位为g
  */
     myIMU.readSensor();
     myIMU.getAccRawValues(&accRaw);
     accdataVal->x=(accRaw.x-accoffsetVal->x)/16384;
     accdataVal->y=(accRaw.y-accoffsetVal->y)/16384;
     accdataVal->z=(accRaw.z-accoffsetVal->z)/16384;
}
void getgyrdata(xyzFloat *gyroffsetVal,xyzFloat *gyrdataVal)
{
  /*
  输出陀螺仪数据单位为m/s2
  */
     myIMU.readSensor();
     myIMU.getGyrRawValues(&gyrRaw);
    gyrdataVal->x=(gyrRaw.x-gyroffsetVal->x)/131;
     gyrdataVal->y=(gyrRaw.y-gyroffsetVal->y)/131;
     gyrdataVal->z=(gyrRaw.z-gyroffsetVal->z)/131;
}

void sendUDPPacket(uint32_t type, uint32_t total, uint32_t index, const uint8_t* data, size_t dataLen) {
  // 构造完整数据包
  uint8_t buffer[sizeof(PacketHeader) + dataLen];
  
  // 填充包头（注意网络字节序转换）
  PacketHeader header;
  header.packetType = (type);     // 主机转网络字节序
  header.totalPackets = (total);
  header.currentIndex = (index);
  
  memcpy(buffer, &header, sizeof(PacketHeader));
  memcpy(buffer + sizeof(PacketHeader), data, dataLen);

  // 发送UDP数据报
  udp.beginPacket(udpAddress, udpPort);
  udp.write(buffer, sizeof(buffer));
  udp.endPacket();
}

void wifitask(void *pvParameters){
  while(1){
    //imu
    imu_x = roll;
    imu_y = pitch;
    imu_z = yaw;
    double imu_data[3] = {imu_x, imu_y, imu_z};
    sendUDPPacket(MSG_IMU_DATA,1,1,(uint8_t*)imu_data,sizeof(imu_data));

    //处理接收
    int packetSize = udp.parsePacket();
    if (packetSize >= sizeof(PacketHeader)) { // 至少包含包头
      // 创建缓冲区
      uint8_t buffer[packetSize];
    
      // 读取完整数据报
      udp.read(buffer, packetSize);
    
      // 解析包头（注意网络字节序转换）
      PacketHeader header;
      memcpy(&header, buffer, sizeof(PacketHeader));
      header.packetType = (header.packetType);
      header.totalPackets = (header.totalPackets);
      header.currentIndex = (header.currentIndex);
    
      // 打印包头信息
      // Serial.printf("Type:%u Total:%u Index:%u\n", 
      //              header.packetType, 
      //              header.totalPackets, 
      //              header.currentIndex);
    
      // 处理数据部分（包头后的内容）
      uint8_t* payload = buffer + sizeof(PacketHeader);
      size_t payloadLength = packetSize - sizeof(PacketHeader);
      // 根据packetType处理不同数据类型（示例）
      switch(header.packetType) {
        case MSG_CONTROL_SIGNALS: 
          double control_signals[4];
          memcpy(control_signals, payload, 4 * sizeof(double));
          // 在这里添加处理控制信号的代码
          can1 = control_signals[0];
          can2 = control_signals[1];
          can3 = control_signals[2];
          can4 = control_signals[3];
          //Serial.print(can1+can2+can3+can4);
          break;
        case MSG_PID_SET_PARAMETERS: 
          
          if(header.currentIndex==1)
            memcpy(pid_params_in, payload, 9 * sizeof(double));
          if(header.currentIndex==0)
            memcpy(pid_params_out, payload, 9 * sizeof(double));
          // Serial.println("接收到PID参数:");
          // for (int i = 0; i < 9; i++) {
          //   Serial.printf("PID内环参数 %d: %lf\n", i + 1, pid_params_in[i]);
          // }
          // for (int i = 0; i < 9; i++) {
          //   Serial.printf("PID外环参数 %d: %lf\n", i + 1, pid_params_out[i]);
          // }
          break;
        case MSG_PID_READ_PARAMETERS: 
          if(header.currentIndex==1)
            sendUDPPacket(MSG_PID_READ_PARAMETERS,1,1,(uint8_t*)pid_params_in,sizeof(pid_params_in));
          if(header.currentIndex==0)
            sendUDPPacket(MSG_PID_READ_PARAMETERS,1,0,(uint8_t*)pid_params_out,sizeof(pid_params_out));
          break;
          // 更多类型处理...
      }
    }
    vTaskDelay(10 / portTICK_PERIOD_MS);  // 延时10毫秒
  }
}

void acctask(void *pvParameters) {
  /*
    加速度计数据获取任务，拿到加速度计数据并且解算角度
  */
    while (1) {
     getaccdata(&accoffset,&accdata);
     roll_acc = atan2(-accdata.x, sqrt(accdata.y*accdata.y + accdata.z*accdata.z))*RAD_TO_DEG;
     pitch_acc = atan2(accdata.y, accdata.z)*RAD_TO_DEG;
     if(IMU_init_flag==false)
     {//将加速度计算出的角度预存，给陀螺仪积分初始使用
      pitch_setup_data=pitch_acc;
      roll_setup_data=roll_acc;
      IMU_init_flag=true;
      Serial.print("pitch_setup_data");
       Serial.println(pitch_setup_data);
       Serial.print("roll_setup_data");
       Serial.println(roll_setup_data);
     }
      vTaskDelay(1 / portTICK_PERIOD_MS);  // 延时1毫秒
    }
}
void gyrtask(void *pvParameters) {
  /*
    陀螺仪数据获取任务，拿到陀螺仪数据并且积分出角度
    右边正，左边负
    上正下负
  */
    while (1) {
      
      unsigned long current_time =millis();
     float dt = (current_time - prev_time) / 1000.0; // 转换为秒
     prev_time = current_time;
      getgyrdata(&gyroffset,&gyrdata);
     if(IMU_init_flag==true)
     {
        pitch_gyro = pitch_setup_data+pitch +gyrdata.x *dt;
        roll_gyro = roll_setup_data+roll + gyrdata.z * dt;
        yaw_gyro =yaw+gyrdata.y * dt;
        pitch_setup_data=0;//初始化结束，数据清零
        roll_setup_data=0;
     }
      vTaskDelay(1 / portTICK_PERIOD_MS); // 延时1毫秒
    }
}
void solvetask(void *pvParameters) {
  /*
    角度融合任务，实现互补滤波
  */
    while (1) {
      pitch = alpha * pitch_gyro + (1 - alpha) * pitch_acc;
      roll = alpha * roll_gyro + (1 - alpha) * roll_acc;
      yaw =yaw_gyro;//yaw暂时没有融合算法，先单放着
      // Serial.print("pitch:");
      //  Serial.print(pitch);
      //  Serial.print(",");
      //  Serial.print("roll:");
      //  Serial.println(roll);
        vTaskDelay(10 / portTICK_PERIOD_MS);  // 延时1秒
    }
}
void vehicletask(void *pvParameters){
  while(1){
  gzkp=pid_params_out[0];
  gzki=pid_params_out[1];
  gzkd=pid_params_out[2];
  fykp=pid_params_out[6];
  fyki=pid_params_out[7];
  fykd=pid_params_out[8];
  #ifdef GUNZHUAN
  aimRoll = (can4-50)*1.8;
  droll = gzPID(aimRoll,roll);
  if (droll<0){    
    num1 = 1000 + can1*6 - droll;
    num2 = 1000 + can1*6 ;       
  }
  if (droll>0){
    num1 = 1000 + can1*6 ;
    num2 = 1000 + can1*6 + droll;   
  }
  #endif
  #ifdef FUYANG
  aimpitch = can2*1.8;
  if(pitch>87.5 && pitch<92.5){
  pos2=90;
  }else{
  dpitch = fyPID(aimpitch,pitch);
  pos2 = 90+dpitch;
  }
  #endif
  #ifndef GUNZHUAN
  num1 = 1000 + can1*6 ;
  num2 = 1000 + can1*6 ;
  #endif
  #ifndef FUYANG
  pos2 = 90;
  #endif
  pos2=limit(pos2,1,179);
  //print_cun(num1,num2,can1,dpitch);
  myservo1.writeMicroseconds(num1);//num1为电机1输入
  myservo2.writeMicroseconds(num2);//num2为电机2输入
  //Serial.println(pos2);
  myservo3.write(pos2);

  if (can3 <75 && can3 > 25){
  pos1 =90;
  myservo4.write(pos1);} 
  if (can3 >75){
  pos1 = 170;
  myservo4.write(pos1);} 
  if (can3 <25){
  pos1 = 10;
  myservo4.write(pos1);

  }
  vTaskDelay(10 / portTICK_PERIOD_MS);  // 延时10毫秒
 }
}



